#ifndef PURE_PURSUIT_CORE_H
#define PURE_PURSUIT_CORE_H

// ROS includes
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning_point.h"
#include "auto_msgs/planning.h"
#include <dynamic_reconfigure/server.h>
#include "pure_pursuit_pkg/purePursuitConfig.h"
// C++ includes
#include <memory>
#include "way_points_helper.h"
#include "common_tool/watchDog.h"
#include "common_tool/clampPid.h"

//读写csv文件所需
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>



namespace pure_pursuit_pkg
{
enum speed_control_mode {SPEED_MODE,TORQUE_MODE};      //转速转距控制模式
enum position_control_mode {NULL_MODE,POSITION_MODE};  //位置控制模式，null代表在规划成完成速度规划，进而控制位置
enum steering_control_mode {AUTO_MODE,MANUAL_MODE};    //转向控制模式ta
class PurePursuit
{
private:
  int deltaNumToStop;//车辆距离指定终点多少路径点时开始减速
  double purePursuitHz;
  //parameter 
  bool linear_interpolate_;  //为假时，预瞄点为预瞄距离外第一个点，真时为预瞄距离外第一个点和
                             //预瞄距离内最后一个点插值
  double hardBrakeDistance;
  double displacement_threshold_;//用于校验有无更踪上
  double relative_angle_threshold_;//用于校验有无更踪上
  int num_of_next_waypoint_;//预瞄距离外第一个点
  int num_of_closet_waypoint_;//距离最近的一个点
  int num_of_destination_waypoint_;//终点最近的一个点
  bool isFindclosetWaypoint;//是否找到了最近点
  bool isFindNextWaypoint;//是否找到下一个点
  bool isDestinationArrive;//是否到达终点
  geometry_msgs::Point position_of_next_target_;//预瞄点

  
  //下面的参数需要事先定好
  double lookahead_distance_;//预瞄距离
  double lookahead_distance_ratio_ =0.6;
  double min_lookahead_distance_=1;
  double max_lookahead_distance_=8;
  double axleBase;//轴距
  void update_lookahead_distance();

  
  //接收消息使用
  geometry_msgs::PoseStamped current_pose_;//机器人当前姿态
 
  geometry_msgs::TwistStamped current_velocity_;//机器人当前twist
  WayPoints current_waypoints_;//规划路径


  double getCmdVelocity(int waypoint) ;//规划速度
  double calcCurvature(geometry_msgs::Point target) ;//计算曲率 
  bool interpolateNextTarget(int next_waypoint, geometry_msgs::Point *next_target);//插值计算预瞄点
  bool verifyFollowing() ;//验证是否跟踪上
  bool searchNextAndClosetPoint();//获取最近的点和预瞄距离以外第一个点
  
public:
    //monitor
  watchDog currentPoseMsgWatchDog;
  watchDog twistMsgWatchDog;
  watchDog wayPointsMsgWatchDog; 
  watchDog destinationPointNumWatchDog; 
  std::ofstream outFile;  
  //controller
  clampPid lateralPid; 
  clampPid speedPidCtr; 
  clampPid positionPidCtr;

  double latral_displacement_=std::numeric_limits<double>::max() ;
  
  //mode control
  speed_control_mode speed_mode;
  position_control_mode position_mode;
  steering_control_mode steering_mode;
  bool speedTestOnly;         //test pid speed ctr
  bool speedPositonTestOnly;  //test speed and postion ctr
  bool PositonTestOnly;       //test  postion ctr and speed ctr is not done by ros
  bool stop;
  bool stopFlag;                  //hard brake or no drive if dont have brake system
  double speedCmd=0;                  //speed cmd under speedTestOnly mode
  PurePursuit(double purePursuitHz_local=50,bool linear_interpolate_mode=true);
  //output
  double tireAngleDeg;
  double norAcc;//归一化加速度
  double norBrake;//归一化制动减速度
  dynamic_reconfigure::Server<pure_pursuit_pkg::purePursuitConfig> server; 
  dynamic_reconfigure::Server<pure_pursuit_pkg::purePursuitConfig>::CallbackType ff;

  void parameterReconfigCB(pure_pursuit_pkg::purePursuitConfig &config,uint32_t level);
  
  ~PurePursuit()
  {

  }


  // for ROS
  void callbackFromCurrentPose(const geometry_msgs::PoseStampedConstPtr &msg);
  void callbackFromCurrentVelocity(const geometry_msgs::TwistStampedConstPtr &msg);
  void callbackFromWayPoints(const styx_msgs::LaneConstPtr &msg);
  void callbackDestinationPointNum(const std_msgs::Int32ConstPtr &msg);

  // for debug
  geometry_msgs::Point getPoseOfNextWaypoint() const
  {
    return current_waypoints_.getWaypointPosition(num_of_next_waypoint_);
  }
  geometry_msgs::Point getPoseOfNextTarget() const
  {
    return position_of_next_target_;
  }
  geometry_msgs::Pose getCurrentPose() const
  {
    return current_pose_.pose;
  }

  double getLookaheadDistance() const
  {
    return lookahead_distance_;
  }
  // processing for ROS
  bool lateralCtr();  
  bool positionCtr(double& tempSpeedCmd);
  bool longtitudeCtr(double& tempAcc);
  bool checkDog();
  void hard_brake();
  bool step();
  void print_to_csv();
  
  //ros wrapper callback function
  void CBLocalizition(const auto_msgs::localizition &msg);
  void CBChassis(const auto_msgs::chassis &msg);
  void CBPlanning(const auto_msgs::planning &msg);
};
}


#endif  // PURE_PURSUIT_CORE_H
